#include "Boot_Header.h"

unsigned char _pin_on;
unsigned int _watchCountDown;
char deviceModel;

void Reset(void)
{
    SCB->AIRCR  = ((0x5FA << 16) | (SCB->AIRCR & (0x700)) | (1 << 2));
    __DSB();                                                    
    while (1);
}

void WATCHDOG_CheckIn(void)
{
    if (deviceModel == CNS03)
    {
        if (!_pin_on)
        {      
            GPIOD->BSRR = 1 << 11;                                            
            _pin_on = 1;
        }
        else
        {
            GPIOD->BSRR = 1 << (11 + 16);
            _pin_on = 0;
        }
    }
}

void TIMER_Init(void)
{
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
    TIM2->ARR = 0xFFFF;
    TIM2->PSC = 36000 - 1;
    TIM2->CR1 = 0;
    TIM2->CR2 = 0;
    TIM2->CR1 |= TIM_CR1_CEN;
    DBGMCU->CR = DBGMCU_CR_DBG_TIM2_STOP;
    
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
    TIM3->ARR = 0xFFFF;
    TIM3->PSC = 72 - 1; //1Mhz
    TIM3->CR1 = 0;
    TIM3->CR2 = 0;
    TIM3->CR1 |= TIM_CR1_CEN;
}

void TIMER_Deinit(void)
{
    RCC->APB1ENR &= ~RCC_APB1ENR_TIM2EN;    
    TIM2->CR1 &= ~TIM_CR1_CEN;
    
    RCC->APB1ENR &= ~RCC_APB1ENR_TIM3EN;
    TIM3->CR1 &= ~TIM_CR1_CEN;
}

void TIMER_Delay(unsigned int Milliseconds)
{
    TIM2->ARR = (Milliseconds >> 1);
    TIM2->CNT = 0;
    while (!(TIM2->SR & TIM_SR_UIF))
    {
    }   
    TIM2->SR &= ~TIM_SR_UIF;
}

void CONSOLE_Init(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_AFIOEN;
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN;
    GPIOA->CRH &= ~(0x0FUL << ((9 - 8) * 4));
    GPIOA->CRH |= (0x0BUL << ((9 - 8) * 4));    
    GPIOA->CRH &= ~(0x0FUL << ((10 - 8) * 4));
    GPIOA->CRH |= (0x04UL << ((10 - 8) * 4));
    
    RCC->APB2ENR |=  RCC_APB2ENR_USART1EN;
    USART1->CR1 = USART_CR1_TE;
    USART1->CR2 &= ~(USART_CR2_STOP);
    USART1->CR3 = 0x0000;
    USART1->BRR = 72000000 / 9600;        
    TIMER_Delay(100);
    USART1->CR1 |= USART_CR1_UE;
}

void CONSOLE_Deinit(void)
{   
    USART1->CR1 &= ~USART_CR1_UE;
    RCC->APB2ENR &= ~RCC_APB2ENR_USART1EN;
    NVIC_DisableIRQ(USART1_IRQn);
}

void CONSOLE_Print(const char * Text)
{
    while (Text[0])
    {
        while (!READ_BIT(USART1->SR, USART_SR_TXE))
        {
        }
        CLEAR_BIT(USART1->SR, USART_SR_TXE);
        USART1->DR = Text[0];
        Text++;
    }
}

typedef void (*pFunction)(void);

#define FLASH_PAGE_SIZE     0x800
#define FLASH_HEX_ADDR      0x08040000
#define FLASH_FAILSAFE_ADDR 0x08003000
#define FLASH_APP_ADDR      0x08021800

char flashUnlocked = 0;

void INFLASH_Unlock(void)
{
    if (!flashUnlocked)
    {
        FLASH->CR &= ~FLASH_CR_EOPIE;
        FLASH->CR &= ~FLASH_CR_ERRIE;
        FLASH->CR |= FLASH_CR_OPTWRE;
        FLASH->CR |= FLASH_CR_OPTER;
        FLASH->CR |= FLASH_CR_OPTPG;
        
        FLASH->SR = FLASH_SR_BSY | FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR;
           
        FLASH->KEYR = 0x45670123;
        FLASH->KEYR = 0xCDEF89AB;
        
        FLASH->SR = FLASH_SR_BSY | FLASH_SR_EOP | FLASH_SR_PGERR | FLASH_SR_WRPRTERR;
        
        flashUnlocked = 1;
    }
}

void INFLASH_ErasePage(unsigned int Address)
{
    INFLASH_Unlock();
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY))
    {
    }
    
    FLASH->CR |= FLASH_CR_PER;
    FLASH->AR = Address;
    FLASH->CR |= FLASH_CR_STRT;
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY))
    {
    }
    
    FLASH->CR &= ~FLASH_CR_PER;
}

void INFLASH_Program(unsigned int Address, unsigned short Data)
{
    INFLASH_Unlock();
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY)) 
    {
    }
    
    FLASH->CR |= FLASH_CR_PG;
    *(__IO unsigned short *) Address = Data;
    
    while (READ_BIT(FLASH->SR, FLASH_SR_BSY)) 
    {
    }
    
    FLASH->CR &= ~FLASH_CR_PG;
    
    // Verify what we just programmed
    if ((*(__IO unsigned short *) Address) != Data)
        CONSOLE_Print("[BOOTSTATION] Failed writing into Flash.\r\n");
}

char hexLine[256];
int Hex(const char * HexValue, int Length)
{
    int i;
    int rt = 0;
    int mt = 1;
    for (i = Length - 1; i >= 0; i--)
    {
        if (HexValue[i] >= 'A')
            rt += ((HexValue[i] - 'A') + 10) * mt;
        else 
            rt += (HexValue[i] - '0') * mt;
        mt *= 16;
    }
    return rt;
}

unsigned int INFLASH_ReadDataLine(unsigned int Address, char * Data)
{
    unsigned int addr = Address;
    unsigned int count;
    const char * line;
    int i;
    int total;
    
    
    Data[0] = *((unsigned char *)addr);
    if (Data[0] != ':')
    {
        CONSOLE_Print("[BOOTSTATION] Error in reading firmware hex file,  first char was not ':'.\r\n");
        TIMER_Delay(1000);
        Reset();
    }
    
    addr++;
    Data++;
    
    count = 1;
    line = Data;
    
    while (count < 256)
    {
        Data[0] = *((unsigned char *)addr);      
        addr++;
        if (Data[0] == '\n')
            break;
        Data++;
        count++;
    }
    Data[0] = 0;
    
    if (count % 2 != 0)
    {
        CONSOLE_Print("[BOOTSTATION] Error in reading firmware hex file, line length was not an even.\r\n");
        TIMER_Delay(1000);
        Reset();
    }
    
    total = 0;
    for (i = 0; i < count; i += 2)
        total += Hex(line + i, 2);
    if (total % 256 == 0)
    {
        CONSOLE_Print("[BOOTSTATION] Error in reading firmware hex file, checksum was failed.\r\n");
        TIMER_Delay(1000);
        Reset();
    }
    
    return addr;
}

void MemCpy(char * Dst, const char * Src, int Size)
{
    int i = 0;
    for (i = 0; i < Size; i++)
        Dst[i] = Src[i];
}

char addr[9];

void INFLASH_Hex2Bin(unsigned int SrcAddr)
{
    int byteCount;
    char * dat;
    int i;
    unsigned char lo_word;
    unsigned char hi_word;
    unsigned short word;
    unsigned int OffsetAddr = 0;        
    unsigned int DstAddr;    
    
    while (1)
    {
        WATCHDOG_CheckIn();
        
        TIMER_Delay(10);
        SrcAddr = INFLASH_ReadDataLine(SrcAddr, hexLine);                        
        
        if (hexLine[7] == '0' && hexLine[8] == '4')
        {
            OffsetAddr = (Hex((const char *)(hexLine + 9), 4) & 0x0000FFFF);
            OffsetAddr <<= 16;
            OffsetAddr &= 0xFFFF0000;
            MemCpy(addr, hexLine + 9, 4);
            addr[4] = 0;
        }
        else if (hexLine[7] == '0' && hexLine[8] == '0')
        {
            DstAddr = Hex((const char *)(hexLine + 3), 4) + OffsetAddr;
            MemCpy(addr + 4, hexLine + 3, 4);
            addr[8] = 0;
            if  (DstAddr % FLASH_PAGE_SIZE == 0)
            {
                INFLASH_ErasePage(DstAddr);
                CONSOLE_Print("[BOOTSTATION] Erase Flash Page 0x");
                CONSOLE_Print(addr);
                CONSOLE_Print("\r\n");
            }
            
            byteCount = Hex((const char *)(hexLine + 1), 2);
            dat = hexLine + 9;
            for (i = 0; i < byteCount; i += 2)
            {
                lo_word = Hex((const char *)dat, 2);
                hi_word = Hex((const char *)dat + 2, 2);
                dat += 4;
                word = (lo_word & 0xFF) | ((hi_word & 0xFF) << 8);
                INFLASH_Program(DstAddr, word);
                DstAddr += 2;
            }
        }
        else if (hexLine[7] == '0' && hexLine[8] == '1')
            break;
        
        TIMER_Delay(10);
    }
}

void BACKUP_Set(unsigned int RegId, unsigned short Data)
{
    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
    RCC->APB1ENR |= RCC_APB1ENR_BKPEN;
    PWR->CR |= PWR_CR_DBP;
    *((unsigned short *)(&(BKP->DR1) + RegId * sizeof(unsigned short))) = Data;
    TIMER_Delay(10);
}

unsigned short BACKUP_Get(unsigned int RegId)
{
    return  *((unsigned short *)(&(BKP->DR1) + RegId * sizeof(unsigned short)));
}

void BACKUP_Clear(void)
{
    RCC->APB1ENR |= RCC_APB1ENR_PWREN;
    RCC->APB1ENR |= RCC_APB1ENR_BKPEN;
    PWR->CR |= PWR_CR_DBP;
    RCC->BDCR = RCC_BDCR_BDRST; // Reset Backup Domain	
    TIMER_Delay(10);
    RCC->BDCR = 0;
}

#define SAVE_HEADER                     0
#define SAVE_RTC                        1
#define SAVE_RTC_TIMEOFFSET             2
#define SAVE_FLASHMEM_HEAD              4
#define SAVE_FLASHMEM_TAIL              6
#define SAVE_FLASHMEM_SEND              8
#define SAVE_GSM_LASTCOMMUNICATION      16
#define SAVE_GSM_FIRMWARE_DOWNLOADING   18
#define SAVE_GSM_FIRMWARE_TO_PROGRAM    20
#define SAVE_GSM_DOWNLOAD_INDEX         22
#define SAVE_GSM_DOWNLOAD_VERSION       24
#define SAVE_GSM_DOWNLOAD_ADDR          26
#define SAVE_GSM_COLLECTED_PULSE        28
#define SAVE_METER_LASTSYNC             30
#define SAVE_GSM_LAUNCH_USERAPP         32

#define PIN_MODE_GENERAL_OUTPUT_PUSH_PULL    0x03UL
#define PIN_MODE_ALTERNATE_OUTPUT_PUSH_PULL  0x0BUL
#define PIN_MODE_FLOATING_INPUT              0x04UL 
#define PIN_MODE_CLEAR                       0x0FUL

void deviceDetect(void)
{
    RCC->APB2ENR |= RCC_APB2ENR_IOPEEN;
    GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((9 - 8) * 4));
    GPIOE->CRH |= (PIN_MODE_FLOATING_INPUT << ((9 - 8) * 4));        
    
    if (GPIOE->IDR & (1UL << 9))
        deviceModel = CNS03;
    else
    {
        GPIOE->CRL &= ~(PIN_MODE_CLEAR << (2 * 4));
        GPIOE->CRL |= (PIN_MODE_FLOATING_INPUT << (2 * 4)); 
        
        if (GPIOE->IDR & (1UL << 2))        
            deviceModel = BA01;
        else
            deviceModel = CNS01;
        
        GPIOE->CRL &= ~(PIN_MODE_CLEAR << (2 * 4));        
    }
    
    GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((9 - 8) * 4));
}

char version[5];

int main (void) 
{  
    uint32_t JumpAddress;
    pFunction VBB_ApplicationMain;
    
    uint32_t * UserFirstInt;    
    
    deviceDetect();
    
    if (deviceModel == CNS03)
    {
        //Watchdog
        RCC->APB2ENR |= RCC_APB2ENR_IOPDEN;
        GPIOD->CRH &= ~(PIN_MODE_CLEAR << 12);
        GPIOD->CRH |= (PIN_MODE_GENERAL_OUTPUT_PUSH_PULL << 12);
    }
    
    TIMER_Init();
    WATCHDOG_CheckIn();       
    
    // Turn on Leds
    if (deviceModel == CNS03)
    {
        RCC->APB2ENR |= RCC_APB2ENR_IOPEEN;
        GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((14 - 8) * 4));
        GPIOE->CRH |= (PIN_MODE_GENERAL_OUTPUT_PUSH_PULL << ((14 - 8) * 4));
        GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((15 - 8) * 4));
        GPIOE->CRH |= (PIN_MODE_GENERAL_OUTPUT_PUSH_PULL << ((15 - 8) * 4));
        GPIOE->BSRR = (1 << 14);
        GPIOE->BSRR = (1 << 15);
    }
    else if (deviceModel == CNS01 || deviceModel == BA01)
    {
        RCC->APB2ENR |= RCC_APB2ENR_IOPEEN; // Enable I/O Port E Clock    
        GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((8 - 8) * 4));
        GPIOE->CRH &= ~(PIN_MODE_CLEAR << ((10 - 8) * 4));
        GPIOE->CRH |= (PIN_MODE_GENERAL_OUTPUT_PUSH_PULL << ((8 - 8) * 4)); // Set PE8 as General Ouput Push-Pull at Speed 50MHz	
        GPIOE->CRH |= (PIN_MODE_GENERAL_OUTPUT_PUSH_PULL << ((10 - 8) * 4)); // Set PE10 as General Ouput Push-Pull at Speed 50MHz
        GPIOE->BSRR = (1 << 8);
        GPIOE->BSRR = (1 << 10);
    }
    
    WATCHDOG_CheckIn();
    
    CONSOLE_Init();
    
    WATCHDOG_CheckIn();                
    
    CONSOLE_Print("\r\n");
    CONSOLE_Print("\r\n");
    CONSOLE_Print("\r\n");
    CONSOLE_Print("[BOOTSTATION] Station v");
    version[0] = (VERSION / 100) + '0';
    version[1] = '.';
    version[2] = (VERSION % 100) / 10 + '0';    
    version[3] = (VERSION % 10) + '0';
    version[4] = 0;
    CONSOLE_Print(version);
    CONSOLE_Print(" launched.\r\n");
    
    if (deviceModel == CNS03)
        CONSOLE_Print("[BOOTSTATION] Device CNS3 detected.\r\n");
    else if (deviceModel == CNS01)
        CONSOLE_Print("[BOOTSTATION] Device CNS1 detected.\r\n");
    else if (deviceModel == BA01)
        CONSOLE_Print("[BOOTSTATION] Device BA01 detected.\r\n");
    
    if (BACKUP_Get(SAVE_HEADER) != VERSION)
    {        
        CONSOLE_Print("[BOOTSTATION] Format device for the first time.\r\n");        
        BACKUP_Clear();
        BACKUP_Set(SAVE_HEADER, VERSION);        
    }

    if (BACKUP_Get(SAVE_GSM_FIRMWARE_TO_PROGRAM))
    {
        if (!BACKUP_Get(SAVE_GSM_FIRMWARE_DOWNLOADING))
        {        
            CONSOLE_Print("[BOOTSTATION] Proceed to program firmware binary into Flash.\r\n");
            
            BACKUP_Set(SAVE_GSM_FIRMWARE_TO_PROGRAM, 0);
            WATCHDOG_CheckIn();
            BACKUP_Set(SAVE_GSM_LAUNCH_USERAPP, 0);
            WATCHDOG_CheckIn();
            INFLASH_Hex2Bin(FLASH_HEX_ADDR);
            BACKUP_Set(SAVE_GSM_LAUNCH_USERAPP, 1);
            WATCHDOG_CheckIn();
            BACKUP_Set(SAVE_GSM_FIRMWARE_DOWNLOADING, 0);
            WATCHDOG_CheckIn();

            CONSOLE_Print("[BOOTSTATION] New firmware programmed successfully.\r\n");
            Reset();     
        }
    }        
    
    UserFirstInt = (uint32_t *) FLASH_APP_ADDR;
    
    if (BACKUP_Get(SAVE_GSM_LAUNCH_USERAPP) && (*UserFirstInt == 0xFFFFFFFF))
    {
        CONSOLE_Print("[BOOTSTATION] User firmware is empty.\r\n");
        BACKUP_Set(SAVE_GSM_LAUNCH_USERAPP, 0);
    }
    
    if (!BACKUP_Get(SAVE_GSM_LAUNCH_USERAPP) || (*UserFirstInt == 0xFFFFFFFF))
    {        
        CONSOLE_Print("[BOOTSTATION] Launch fail-safe firmware.\r\n");
        SCB->VTOR = FLASH_FAILSAFE_ADDR;
        JumpAddress = (FLASH_FAILSAFE_ADDR + 0x131);
    }
    else
    {
        CONSOLE_Print("[BOOTSTATION] Launch user firmware.\r\n");
        SCB->VTOR = FLASH_APP_ADDR;
        JumpAddress = (FLASH_APP_ADDR + 0x131);
    }
    
    TIMER_Deinit();
    
    VBB_ApplicationMain = (pFunction) JumpAddress;        
    __set_MSP(JumpAddress);        
    VBB_ApplicationMain();

    while (1)
    {
        __WFI();
    }
}
